#define TRUE	1
#define FALSE	0

#define S1 		SIU.GPDI[64].R
#define S2 		SIU.GPDI[65].R
#define S3 		SIU.GPDI[66].R
#define S4 		SIU.GPDI[67].R

#define PE0_64		64
#define PE1_65		65
#define PE2_66		66
#define PE3_67		67
#define PE4_68		68
#define PE5_69		69
#define PE6_70		70
#define PE7_71		71
	
#define SetPinAsInput(pinPcr)			( SIU.PCR[pinPcr].R=0x0100 )		//configuracion como bit de entrada  SIU.PCR[64].B.IBE=1
#define SetPinAsOutput(pinPcr)			( SIU.PCR[pinPcr].B.OBE=TRUE )		//configuracion como bit de salida

#define SetPinAs0(pinPcr)			( SIU.GPDO[pinPcr].B.PDO=FALSE )				
#define SetPinAs1(pinPcr)			( SIU.GPDO[pinPcr].B.PDO=TRUE )
/*-------------------------------------------------------------------------------------------------
		INICIALIZACIONES_mias																		
-------------------------------------------------------------------------------------------------*/
void inicializacion_PUSH_B(void)
{	
	SetPinAsInput(PE0_64);
	SetPinAsInput(PE1_65);
	SetPinAsInput(PE2_66);
	SetPinAsInput(PE3_67);
}
void retardo_ms(unsigned int msegs )
{
	volatile int i=0,t=0;
	while (msegs)
	{
        for(i=0;i<=8000;i++)
        {
	   		t++;
	    }	    
	   msegs--;
	}
}
void retardo_us(unsigned int usegs )//retardo_us(20)=15.5KHz
{
	volatile int i=0,t=0;	
	while (usegs)
	{
        for(i=0;i<=8;i++)
        {
	   		t++;
	    }	    
	   usegs--;
	}
}
/*-------------------------------------------------------------------------------------------------
		INICIALIZACIONES DEL MICRO-CONTROLADOR																		
-------------------------------------------------------------------------------------------------*/
void initModesAndClock(void) 
{
  ME.MER.R = 0x0000001D;          /* Enable DRUN, RUN0, SAFE, RESET modes */
                                  /* Initialize PLL before turning it on: */
/* Use 1 of the next 2 lines depending on crystal frequency: */
  CGM.FMPLL_CR.R = 0x02400100;    /* 8 MHz xtal: Set PLL0 to 64 MHz */   
/*CGM.FMPLL_CR.R = 0x12400100;*/  /* 40 MHz xtal: Set PLL0 to 64 MHz */   
  ME.RUN[0].R = 0x001F0074;       /* RUN0 cfg: 16MHzIRCON,OSC0ON,PLL0ON,syclk=PLL */
  ME.RUNPC[1].R = 0x00000010; 	  /* Peri. Cfg. 1 settings: only run in RUN0 mode */
  ME.PCTL[32].R = 0x01; 		  /* MPC56xxB ADC 0: select ME.RUNPC[1] */
  ME.PCTL[57].R = 0x01; 		  /* MPC56xxB CTUL: select ME.RUNPC[1] */
  ME.PCTL[68].R = 0x01;           /* MPC56xxB/S SIUL:  select ME.RUNPC[0] */	
  ME.PCTL[72].R = 0x01;           /* MPC56xxB/S EMIOS 0:  select ME.RUNPC[0] */	
                                  /* Mode Transition to enter RUN0 mode: */
  ME.MCTL.R = 0x40005AF0;         /* Enter RUN0 Mode & Key */
  ME.MCTL.R = 0x4000A50F;         /* Enter RUN0 Mode & Inverted Key */  
  while (ME.GS.B.S_MTRANS) {}     /* Wait for mode transition to complete */    
                                  /* Note: could wait here using timer and/or I_TC IRQ */
  while(ME.GS.B.S_CURRENTMODE != 4) {} /* Verify RUN0 is the current mode */
}
void initPeriClkGen(void)
{
  CGM.SC_DC[2].R = 0x80;          /* MPC56xxB: Enable peri set 3 sysclk divided by 1 */
}
void disableWatchdog(void)
{
  SWT.SR.R = 0x0000c520;     /* Write keys to clear soft lock bit */
  SWT.SR.R = 0x0000d928; 
  SWT.CR.R = 0x8000010A;     /* Clear watchdog enable (WEN) */
}        
/*------------------------------------------------------------------------------------------------*/
#define  INITIAL_POSITION_RA     0x3AA3 //15011
#define  INITIAL_POSITION_RB     0x4114 //16660
#define  TURN_LEFT_RA            0x3C0A //14723
#define  TURN_RIGHT_RA           0x3983 //15370
/*------------------------------------------------------------------------------------------------*/
void inicializaciones_MicroControlador(void)
{
	initModesAndClock(); /* Initialize mode entries and system clock */
  	initPeriClkGen();    /* Initialize peripheral clock generation for DSPIs */
  	disableWatchdog();   /* Disable watchdog */
}
/*-------------------------------------------------------------------------------------------------
		INICIALIZACIONES PWM EMIOS_0																		
-------------------------------------------------------------------------------------------------*/
#define ValvuePWM_EmiosCH22_A  			EMIOS_0.CH[22].CADR.R
#define ValvuePWM_EmiosCH21_A  			EMIOS_0.CH[21].CADR.R
#define ValvuePWM_EmiosCH22_B  			EMIOS_0.CH[22].CBDR.R
#define ValvuePWM_EmiosCH21_B  			EMIOS_0.CH[21].CBDR.R
#define ValvuePWM_EmiosCH6_A  			EMIOS_0.CH[6].CADR.R
#define ValvuePWM_EmiosCH7_A  			EMIOS_0.CH[7].CADR.R
#define ValvuePWM_EmiosCH6_B  			EMIOS_0.CH[6].CBDR.R
#define ValvuePWM_EmiosCH7_B  			EMIOS_0.CH[7].CBDR.R

#define CH22  		 22
#define CH21   		 21
#define CH6  		 6
#define CH7   		 7

#define ValvuePWM_EmiosCHx_A(pinPcr,Valor)			( EMIOS_0.CH[pinPcr].CADR.R = Valor)
#define ValvuePWM_EmiosCHx_B(pinPcr,Valor)			( EMIOS_0.CH[pinPcr].CBDR.R = Valor)
/*------------------------------------------------------------------------------------------------*/
void initEMIOS_0(void)/* Initialize eMIOS 0 to provide 1 MHz clock to channels */
{
  EMIOS_0.MCR.B.GPRE= 63;   /* Divide 64 MHz sysclk by 63+1 = 64 for 1MHz eMIOS clk*/
  EMIOS_0.MCR.B.GPREN = 1;	/* Enable eMIOS clock */
  EMIOS_0.MCR.B.GTBE = 1;   /* Enable global time base */
  EMIOS_0.MCR.B.FRZ = 1;    /* Enable stopping channels when in debug mode */
}
void initEMIOS_0ch23(void)/* Initialize eMIOS 0 channel 23 as modulus counter*/
{ /* EMIOS 0 CH 23: Modulus Up Counter */
  EMIOS_0.CH[23].CADR.R = 334;      /* Period will be 999+1 = 1000 clocks (1 msec) */
  EMIOS_0.CH[23].CCR.B.MODE = 0x50; /* Modulus Counter Buffered (MCB) */
  EMIOS_0.CH[23].CCR.B.BSL = 0x3;	/* Use internal counter */
  EMIOS_0.CH[23].CCR.B.UCPRE=0;	    /* Set channel prescaler to divide by 1 */
  EMIOS_0.CH[23].CCR.B.UCPEN = 1;   /* Enable prescaler; uses default divide by 1 */
  EMIOS_0.CH[23].CCR.B.FREN = 1; 	/* Freeze channel counting when in debug mode */ 
}
void initEMIOS_0ch21(void)/* Initialize eMIOS 0 channel 0 as OPWM, using ch 23 as time base */
{ /* EMIOS 0 CH 21: Output Pulse Width Modulation */
  EMIOS_0.CH[21].CADR.R = 50;       /* Leading edge when channel counter bus=250*/
  EMIOS_0.CH[21].CBDR.R = 300;      /* Trailing edge when channel counter bus=500*/
  EMIOS_0.CH[21].CCR.B.BSL = 0x0;	/* Use counter bus A (default) */
  EMIOS_0.CH[21].CCR.B.EDPOL = 1;	/* Polarity-leading edge sets output/trailing clears*/
  EMIOS_0.CH[21].CCR.B.MODE = 0x60; /* Mode is OPWM Buffered */
  SIU.PCR[69].R = 0x0600;           /* MPC56xxS: Assign EMIOS_0 ch 21 to pad */
}
void initEMIOS_0ch22(void)/* Initialize eMIOS 0 channel 1 as OPWM, using ch 23 as time base */
{ /* EMIOS 0 CH 22: Output Pulse Width Modulation */
  EMIOS_0.CH[22].CADR.R = 200;      /* Leading edge when channel counter bus=500*/
  EMIOS_0.CH[22].CBDR.R = 300;      /* Trailing edge when channel's counter bus=999*/
  EMIOS_0.CH[22].CCR.B.BSL = 0x0;	/* Use counter bus A (default) */
  EMIOS_0.CH[22].CCR.B.EDPOL = 1;	/* Polarity-leading edge sets output/trailing clears*/
  EMIOS_0.CH[22].CCR.B.MODE = 0x60; /* Mode is OPWM Buffered */
  SIU.PCR[70].R = 0x0600;           /* MPC56xxS: Assign EMIOS_0 ch 22 to pad */
}
void initEMIOS_0ch6(void)/* Initialize eMIOS 0 channel 0 as OPWM, using ch 23 as time base */
{ /* EMIOS 0 CH 21: Output Pulse Width Modulation */
  EMIOS_0.CH[6].CADR.R = 100;       /* Leading edge when channel counter bus=250*/
  EMIOS_0.CH[6].CBDR.R = 300;       /* Trailing edge when channel counter bus=500*/
  EMIOS_0.CH[6].CCR.B.BSL = 0x0;	/* Use counter bus A (default) */
  EMIOS_0.CH[6].CCR.B.EDPOL = 1;	/* Polarity-leading edge sets output/trailing clears*/
  EMIOS_0.CH[6].CCR.B.MODE = 0x60;  /* Mode is OPWM Buffered */
  SIU.PCR[30].R = 0x0600;/*PB[14]*/ /* MPC56xxS: Assign EMIOS_0 ch 21 to pad */
}

void initEMIOS_0ch0(void)
{ /* EMIOS 0 CH 0: Modulus Up Counter */  
  EMIOS_0.CH[0].CADR.R = 14284;    /* Period will be 13549+1 = 13550 clocks (13.55 msec) */
  EMIOS_0.CH[0].CCR.B.MODE = 0x50; /* Modulus Counter Buffered (MCB) */
  EMIOS_0.CH[0].CCR.B.BSL = 0x1;   /* Use counter bus B */
  EMIOS_0.CH[0].CCR.B.UCPRE=0;     /* Set channel prescaler to divide by 1 */
  EMIOS_0.CH[0].CCR.B.UCPEN = 1;   /* Enable prescaler; uses default divide by 1 */
  EMIOS_0.CH[0].CCR.B.FREN = 1;    /* Freeze channel counting when in debug mode */
}
void initEMIOS_0ch7(void)/* Initialize eMIOS 0 channel 1 as OPWM, using ch 23 as time base */
{ /* EMIOS 0 CH 22: Output Pulse Width Modulation */
  EMIOS_0.CH[7].CADR.R = 12711;     /* Leading edge when channel counter bus=500*/
  EMIOS_0.CH[7].CBDR.R = 14284;     /* Trailing edge when channel's counter bus=999*/
  EMIOS_0.CH[7].CCR.B.BSL = 0x1;    /* Use counter bus B */
  EMIOS_0.CH[7].CCR.B.EDPOL = 1;	/* Polarity-leading edge sets output/trailing clears*/
  EMIOS_0.CH[7].CCR.B.MODE = 0x60;  /* Mode is OPWM Buffered */
  SIU.PCR[31].R = 0x0600;/*PB[15]*/ /* MPC56xxS: Assign EMIOS_0 ch 22 to pad */
}
/*-------------------------------------------------------------------------------------------------
		INICIALIZACIONES ADC																		
-------------------------------------------------------------------------------------------------*/
#define ValvueADC_ANS0 				ADC.CDR[32].B.CDATA		/* Read ANS0 conversion result data */
#define ValvueADC_ANS1 				ADC.CDR[33].B.CDATA		/* Read ANS1 conversion result data */
#define ValvueADC_ANS2 				ADC.CDR[34].B.CDATA		/* Read ANS2 conversion result data */
#define WaitADC_ScanComplet 		ADC.CDR[33].B.VALID != 1
/*------------------------------------------------------------------------------------------------*/
void init_ADC(void) 
{
	ADC.MCR.R = 0x20000000; 		/* Initialize ADC0 for scan mode */
	ADC.NCMR[1].R = 0x00000007; 	/* Select ANS0:2 inputs for normal conversion */
	ADC.CTR[1].R = 0x00008606; 		/* Conversion times for 32MHz ADClock */
	
	ADC.MCR.B.NSTART=1;             /* Trigger normal conversions for ADC0 */
	
	SIU.PCR[24].R = 0x2000;/*PB[8]*//* MPC56xxB: Initialize PB[8] as ANS0 */
	SIU.PCR[25].R = 0x2000; 		/* MPC56xxB: Initialize PB[9] as ANS1 */
	SIU.PCR[26].R = 0x2000; 		/* MPC56xxB: Initialize PB[10] as ANS2 */                    			
}
/*-------------------------------------------------------------------------------------------------
			CAMARA_LINEAL!!!!!																		
-------------------------------------------------------------------------------------------------*/
#define PC14_14			46		//SI
#define PC15_15			47		//CLK

#define SI				PC14_14
#define CLK				PC15_15		
#define Tclk			20		//tiempo en alto y bajo de CLK en us

#define ValorMedio 		0x40

unsigned int   ValorADC[129];	//valores del ADC por pixel
unsigned char  ValorCam[129];	//comparacion de 0 o 1
unsigned char  Grupo[11];		//daots de las sumas de las comparaciones
unsigned char  i;				//incrementos necesarios
unsigned char  j;				//incrementos necesarios
/*------------------------------------------------------------------------------------------------*/
void Get_ADCconversion(void)
{
	while(WaitADC_ScanComplet){};
	ValorADC[i] = ValvueADC_ANS0;
}
void CLK_Camara(void)
{
	SetPinAs1(CLK);		//CLK set 1
	retardo_us(Tclk);	//tiempo en alto
	SetPinAs0(CLK);		//CLK set 0
	retardo_us(Tclk);	//tiempo en bajo	
}
void CamInitPin(void)
{
	SetPinAsOutput(CLK);	//PA14 como salida
	SetPinAsOutput(SI);		//PA15 como salida	
	SetPinAs0(CLK);			//inicial Px set 0
	SetPinAs0(SI);			//inicial Px set 0	
}
void SI_CamaraInit(void)
{	
	CLK_Camara();			//un periodo CLK
	SetPinAs1(CLK);			//CLK set 1
	retardo_us(Tclk);		//tiempo en alto
	SetPinAs0(CLK);			//CLK set 0
	retardo_us(1); 			//tiempo para incicial SI
	SetPinAs1(SI);			//SI set 1
	retardo_us(Tclk-1); 	//tiempo restante en bajo
}
void CLK_CamaraGetPix(unsigned char veces)
{
	i=0;						//inicializamos el arreglo del ADC	
	while(veces)
	{
		SetPinAs1(CLK);			//CLK set 1
		SetPinAs0(SI);			//SI set 0, por primera vez que entre deja en 0 SI
		retardo_us(Tclk);		//tiempo en alto		
		SetPinAs0(CLK);			//CLK set 0
		Get_ADCconversion();	//obtener valor ADC en flanco de bajada
		retardo_us(Tclk);		//tiempo en bajo
		
		veces--;
		i++;	
	}		
}
void Codificacion(void)
{
	for(i=18;i<=128;i++)		//tener en cuenta solo 110 pixeles
	{
		if(ValorADC[i]<=ValorMedio)	//comparar el valor del ADC y decidir si es 0 o 1
		{
			ValorCam[j]=0;		//guardar un 0 en el ValorCam i	
		}
		else
		{
			ValorCam[j]=1;		//guardar un 1 en el ValorCami	
		}					
		j++;
	}	
	
	i=0;
	for(j=0;j<=10;j++)			//suma de los valores y guardarlos en los grupos			
	{
		Grupo[i]=ValorCam[j]+ValorCam[j+1]+ValorCam[j+2]+ValorCam[j+3]+ValorCam[j+4]+ValorCam[j+5]+ValorCam[j+6]+ValorCam[j+7]+ValorCam[j+8]+ValorCam[j+9];
		i++;
	}	
}
void PosicionServoVelocidadDC(void)
{
	if(Grupo[0]!=10 && Grupo[1]==10 && Grupo[2]==10 && Grupo[3]==10 && Grupo[4]==10 && Grupo[5]==10 && Grupo[6]==10 && Grupo[7]==10 && Grupo[8]==10 && Grupo[9]==10 && Grupo[10]==10)
	{
		ValvuePWM_EmiosCHx_A(CH7,12411);	
	}
	else if(Grupo[0]==10 && Grupo[1]!=10 && Grupo[2]==10 && Grupo[3]==10 && Grupo[4]==10 && Grupo[5]==10 && Grupo[6]==10 && Grupo[7]==10 && Grupo[8]==10 && Grupo[9]==10 && Grupo[10]==10)
	{
		ValvuePWM_EmiosCHx_A(CH7,12471);		
	}
	else if(Grupo[0]==10 && Grupo[1]==10 && Grupo[2]!=10 && Grupo[3]==10 && Grupo[4]==10 && Grupo[5]==10 && Grupo[6]==10 && Grupo[7]==10 && Grupo[8]==10 && Grupo[9]==10 && Grupo[10]==10)
	{
		ValvuePWM_EmiosCHx_A(CH7,12531);	
	}
	else if(Grupo[0]==10 && Grupo[1]==10 && Grupo[2]==10 && Grupo[3]!=10 && Grupo[4]==10 && Grupo[5]==10 && Grupo[6]==10 && Grupo[7]==10 && Grupo[8]==10 && Grupo[9]==10 && Grupo[10]==10)
	{
		ValvuePWM_EmiosCHx_A(CH7,12591);	
	}
	else if(Grupo[0]==10 && Grupo[1]==10 && Grupo[2]==10 && Grupo[3]==10 && Grupo[4]!=10 && Grupo[5]==10 && Grupo[6]==10 && Grupo[7]==10 && Grupo[8]==10 && Grupo[9]==10 && Grupo[10]==10)
	{
		ValvuePWM_EmiosCHx_A(CH7,12651);	
	}
	else if(Grupo[0]==10 && Grupo[1]==10 && Grupo[2]==10 && Grupo[3]==10 && Grupo[4]==10 && Grupo[5]!=10 && Grupo[6]==10 && Grupo[7]==10 && Grupo[8]==10 && Grupo[9]==10 && Grupo[10]==10)
	{
		ValvuePWM_EmiosCHx_A(CH7,12711);	
	}
	else if(Grupo[0]==10 && Grupo[1]==10 && Grupo[2]==10 && Grupo[3]==10 && Grupo[4]==10 && Grupo[5]==10 && Grupo[6]!=10 && Grupo[7]==10 && Grupo[8]==10 && Grupo[9]==10 && Grupo[10]==10)
	{
		ValvuePWM_EmiosCHx_A(CH7,12771);	
	}
	else if(Grupo[0]==10 && Grupo[1]==10 && Grupo[2]==10 && Grupo[3]==10 && Grupo[4]==10 && Grupo[5]==10 && Grupo[6]==10 && Grupo[7]!=10 && Grupo[8]==10 && Grupo[9]==10 && Grupo[10]==10)
	{
		ValvuePWM_EmiosCHx_A(CH7,12831);	
	}
	else if(Grupo[0]==10 && Grupo[1]==10 && Grupo[2]==10 && Grupo[3]==10 && Grupo[4]==10 && Grupo[5]==10 && Grupo[6]==10 && Grupo[7]==10 && Grupo[8]!=10 && Grupo[9]==10 && Grupo[10]==10)
	{
		ValvuePWM_EmiosCHx_A(CH7,12891);	
	}
	else if(Grupo[0]==10 && Grupo[1]==10 && Grupo[2]==10 && Grupo[3]==10 && Grupo[4]==10 && Grupo[5]==10 && Grupo[6]==10 && Grupo[7]==10 && Grupo[8]==10 && Grupo[9]!=10 && Grupo[10]==10)
	{
		ValvuePWM_EmiosCHx_A(CH7,12951);	
	}
	else if(Grupo[0]==10 && Grupo[1]==10 && Grupo[2]==10 && Grupo[3]==10 && Grupo[4]==10 && Grupo[5]==10 && Grupo[6]==10 && Grupo[7]==10 && Grupo[8]==10 && Grupo[9]==10 && Grupo[10]!=10)
	{
		ValvuePWM_EmiosCHx_A(CH7,13011);	
	}		
}






